#ifndef PObH_H
#define PObH_H

#include <ostream>
#include <vector>


void display_scene(int argc, char* argv[]);
void single_robot_optimization_example(int argc, char* argv[], int num_path=4);


using namespace std;
/*
template<class RefPoint2D> // RefPoint2D should have location and rotation
ostream& operator<<(ostream& os,vector<RefPoint2D> path)
{
	os << "1" << endl;
  long long size=path.size();
  os << to_string(size) << endl;
  for (unsigned int i = 0; i < path.size(); i++)
  {
	   os << path[i] << endl;  
  }  
  return os;
};
*/
template<class RefPoint2D> // RefPoint2D should have location and rotation
ostream& operator<<(ostream& os,vector<RefPoint2D> path)
{
	os << "1" << endl;
  long long size=path.size();
  os << to_string(size) << endl;
  RefPoint2D p0 = path[0];

  double x = CGAL::to_double(p0.get_location().x());
  double y = CGAL::to_double(p0.get_location().y());
  double a = p0.get_rotation().to_angle();
	  
  os << std::to_string((long double)x);
  os << " " << std::to_string((long double)y); 
  os << " " << std::to_string((long double)a);
  os << endl;
  for (unsigned int i = 1; i < path.size(); i++)
  {
	  RefPoint2D p1 = path[i-1];
	  RefPoint2D p2 = path[i];
	  
	  double x1 = CGAL::to_double(p1.get_location().x());
	  double x2 = CGAL::to_double(p2.get_location().x());
	  double y1 = CGAL::to_double(p1.get_location().y());
	  double y2 = CGAL::to_double(p2.get_location().y());
	  double a1 = p1.get_rotation().to_angle();
	  double a2 = p2.get_rotation().to_angle();
	  os << std::to_string((long double)x2-x1);
	  os << " " << std::to_string((long double)y2-y1); 
	  os << " " << std::to_string((long double)a2-a1);
	  os << endl;
  }  
  return os;
};

#endif //PObH_H